/*
 * Copyright (c) 2006-2021, RT-Thread Development Team
 *
 * SPDX-License-Identifier: Apache-2.0
 *
 * Change Logs:
 * Date           Author       Notes
 * 2024-07-08     86188       the first version
 */

#include "stm32f4xx_hal.h"
#include <inc/pid.h>

#define PID_UPDATE_RATE 10   ///10hz
#define PID_UPDATE_DT   (1.0f/PID_UPDATE_RATE)


//float VEL_KP = 15.5f;
//float VEL_KI = 70.0f;
//float VEL_KD = 0.0f;

float VEL_KP = 15.5f;
float VEL_KI = 15.02f;
float VEL_KD = 0.00f;



float POSX_KP = 0.2f;
float POSX_KI = 0.0f;
float POSX_KD = 0.1f;

float POSY_KP = 0.13f;
float POSY_KI = 0.0f;
float POSY_KD = 0.06f;


float POSZ_KP = 0.5f;
float POSZ_KI = 0.0f;
float POSZ_KD = 0.15f;

PidObject leftFrontPid;
PidObject rightFrontPid;
PidObject leftBackPid;
PidObject rightBackPid;



PidObject xPosPid;
PidObject yPosPid;
PidObject zPosPid;


void abs_outlimit(float *a, float ABS_MAX)
{
    if(*a > ABS_MAX)
        *a = ABS_MAX;
    if(*a < -ABS_MAX)
        *a = -ABS_MAX;
}


void pidSetIntegralLimit(PidObject* pid, const float limit)
{
    pid->iLimit = limit;
        pid->iLimitLow = -limit;
}
void pidSetOutLimit(PidObject* pid, const float maxoutput)
{
    pid->maxOutput = maxoutput;
}
void pidReset(PidObject* pid)
{
    pid->error     = 0;
    pid->prevError = 0;
    pid->integ     = 0;
    pid->deriv     = 0;
    pid->desired   = 0;
}
void pidSetPID(PidObject* pid, const float kp,const float ki,const float kd)
{
    pid->kp = kp;
    pid->ki = ki;
    pid->kd = kd;
}


void pidUpdate(PidObject *pid,const float error)
{

    pid->error = error;
    pid->integ += pid->error * PID_UPDATE_DT;
    pid->deriv = (pid->error - pid->prevError) / PID_UPDATE_DT;

    pid->outP = pid->kp * pid->error;
    pid->outI = pid->ki * pid->integ;
    pid->outD = pid->kd * pid->deriv;

    abs_outlimit(&(pid->integ), pid->iLimit);

    pid->out = pid->outP + pid->outI + pid->outD;

    abs_outlimit(&pid->out, pid->maxOutput);

    pid->prevError = pid->error;


}




void velocityPidUpdata(PidObject* pid,float error)
{
    pid->error = error;

    pid->out += pid->kp * (pid->error - pid->prevError) + pid->ki * pid->error;

    if(pid->out > pid->maxOutput)
        pid->out = pid->maxOutput;
    else if(pid->out < -pid->maxOutput)
        pid->out = -pid->maxOutput;

    pid->prevError = pid->error;


}


